{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Stanley实现轨迹跟踪\n",
    "相关理论知识参考[博客](https://blog.csdn.net/weixin_42301220/article/details/124899547?spm=1001.2014.3001.5501)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "假设无人车模型如下\n",
    "\n",
    "![在这里插入图片描述](https://img-blog.csdnimg.cn/98de36e913bd4fcd86b4f3ac933b0afc.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 106,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib\n",
    "import matplotlib.pyplot as plt\n",
    "# %matplotlib inline\n",
    "# %matplotlib notebook\n",
    "%matplotlib qt5\n",
    "# %matplotlib auto\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 107,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "\n",
    "\n",
    "class KinematicModel_3:\n",
    "  \"\"\"假设控制量为转向角delta_f和加速度a\n",
    "  \"\"\"\n",
    "\n",
    "  def __init__(self, x, y, psi, v, L, dt):\n",
    "    self.x = x\n",
    "    self.y = y\n",
    "    self.psi = psi\n",
    "    self.v = v\n",
    "    self.L = L\n",
    "    # 实现是离散的模型\n",
    "    self.dt = dt\n",
    "\n",
    "  def update_state(self, a, delta_f):\n",
    "    self.x = self.x+self.v*math.cos(self.psi)*self.dt\n",
    "    self.y = self.y+self.v*math.sin(self.psi)*self.dt\n",
    "    self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt\n",
    "    self.v = self.v+a*self.dt\n",
    "\n",
    "  def get_state(self):\n",
    "    return self.x, self.y, self.psi, self.v\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "参数设置"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 108,
   "metadata": {},
   "outputs": [],
   "source": [
    "k=0.2 # 增益系数\n",
    "dt=0.1 # 时间间隔，单位：s\n",
    "\n",
    "L=2 # 车辆轴距，单位：m\n",
    "v = 2 # 初始速度\n",
    "x_0=0 # 初始x\n",
    "y_0=-3 #初始y\n",
    "psi_0=0 # 初始航向角"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "搜索目标临近点"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 109,
   "metadata": {},
   "outputs": [],
   "source": [
    "def cal_target_index(robot_state, refer_path):\n",
    "    \"\"\"得到临近的路点\n",
    "\n",
    "    Args:\n",
    "        robot_state (_type_): 当前车辆位置\n",
    "        refer_path (_type_): 参考轨迹（数组）\n",
    "\n",
    "    Returns:\n",
    "        _type_: 最近的路点的索引\n",
    "    \"\"\"\n",
    "    dists = []\n",
    "    for xy in refer_path:\n",
    "        dis = np.linalg.norm(robot_state-xy)\n",
    "        dists.append(dis)\n",
    "\n",
    "    min_index = np.argmin(dists)\n",
    "    return min_index\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "角度归一化到[-pi,pi]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 110,
   "metadata": {},
   "outputs": [],
   "source": [
    "def normalize_angle(angle):\n",
    "    \"\"\"\n",
    "    Normalize an angle to [-pi, pi].\n",
    "\n",
    "    :param angle: (float)\n",
    "    :return: (float) Angle in radian in [-pi, pi]\n",
    "    copied from https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/stanley_control/stanley_control.html\n",
    "    \"\"\"\n",
    "    while angle > np.pi:\n",
    "        angle -= 2.0 * np.pi\n",
    "\n",
    "    while angle < -np.pi:\n",
    "        angle += 2.0 * np.pi\n",
    "\n",
    "    return angle\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Stanley 控制器"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 111,
   "metadata": {},
   "outputs": [],
   "source": [
    "def stanley_control(robot_state,refer_path, refer_path_psi):\n",
    "    \"\"\"stanley控制\n",
    "\n",
    "    Args:\n",
    "        robot_state (_type_): 机器人位姿，包括x,y,yaw,v\n",
    "        refer_path (_type_): 参考轨迹的位置\n",
    "        refer_path_psi (_type_): 参考轨迹上点的切线方向的角度\n",
    "        last_target_index (_type_): 上一个目标临近点\n",
    "\n",
    "    Returns:\n",
    "        _type_: _description_\n",
    "    \"\"\"\n",
    "    current_target_index = cal_target_index(robot_state[0:2],refer_path)\n",
    "\n",
    "    \n",
    "    # 当计算出来的目标临近点索引大于等于参考轨迹上的最后一个点索引时\n",
    "    if current_target_index>=len(refer_path):  \n",
    "        current_target_index=len(refer_path)-1 \n",
    "        current_ref_point = refer_path[-1] \n",
    "        psi_t = refer_path_psi[-1]\n",
    "    else:\n",
    "        # print(current_target_index)\n",
    "        current_ref_point=refer_path[current_target_index]\n",
    "        psi_t = refer_path_psi[current_target_index]\n",
    "    \n",
    "    # 计算横向误差e_y\n",
    "    # 1. 参考自https://blog.csdn.net/renyushuai900/article/details/98460758\n",
    "    # if(robot_state[0]-current_ref_point[0])*psi_t-(robot_state[1]-current_ref_point[1])>0:\n",
    "    # 2. \n",
    "    if(robot_state[1]-current_ref_point[1])*math.cos(psi_t)-(robot_state[0]-current_ref_point[0])*math.sin(psi_t)<=0:\n",
    "\n",
    "        e_y=np.linalg.norm(robot_state[0:2]-current_ref_point)\n",
    "    else:\n",
    "        e_y = -np.linalg.norm(robot_state[0:2]-current_ref_point)\n",
    "\n",
    "\n",
    "    # 通过公式(5)计算转角,符号保持一致\n",
    "    psi = robot_state[2]\n",
    "    v = robot_state[3]\n",
    "    # psi_t的计算我看还有直接这么计算的\n",
    "    # psi_t = math.atan2(current_ref_point[1]-robot_state[1],current_ref_point[0]-robot_state[0])\n",
    "    theta_e = psi_t-psi\n",
    "    delta_e = math.atan2(k*e_y,v)\n",
    "    delta = normalize_angle(theta_e+delta_e)\n",
    "    return delta,current_target_index\n",
    "\n",
    "    \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "主函数"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 112,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "from celluloid import Camera # 保存动图时用，pip install celluloid\n",
    "# set reference trajectory\n",
    "refer_path = np.zeros((1000, 2))\n",
    "refer_path[:, 0] = np.linspace(0, 100, 1000)  # 直线\n",
    "refer_path[:, 1] = 2*np.sin(refer_path[:, 0]/3.0) +  2.5*np.cos(refer_path[:, 0]/2.0)  # 生成正弦轨迹\n",
    "refer_path_psi = [math.atan2(refer_path[i+1,1]-refer_path[i,1],refer_path[i+1,0]-refer_path[i,0]) for i in range(len(refer_path)-1)] # 参考轨迹上点的切线方向的角度,近似计算\n",
    "\n",
    "# 运动学模型\n",
    "ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt)\n",
    "goal = refer_path[-2]\n",
    "\n",
    "x_ = []\n",
    "y_ = []\n",
    "fig = plt.figure(1)\n",
    "# 保存动图用\n",
    "camera = Camera(fig)\n",
    "# plt.ylim([-3,3])\n",
    "for _ in range(500):\n",
    "    robot_state = np.zeros(4)\n",
    "    robot_state[0] = ugv.x\n",
    "    robot_state[1] = ugv.y\n",
    "    robot_state[2]=ugv.psi\n",
    "    robot_state[3]=ugv.v\n",
    "\n",
    "\n",
    "    delta,ind = stanley_control(robot_state,refer_path,refer_path_psi)\n",
    "\n",
    "    ugv.update_state(0, delta)  # 加速度设为0，恒速\n",
    "\n",
    "    x_.append(ugv.x)\n",
    "    y_.append(ugv.y)\n",
    "\n",
    "    # 显示动图\n",
    "    plt.cla()\n",
    "    plt.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0)\n",
    "    plt.plot(x_, y_, \"-r\", label=\"trajectory\")\n",
    "    plt.plot(refer_path[ind,0], refer_path[ind,1], \"go\", label=\"target\")\n",
    "    # plt.axis(\"equal\")\n",
    "    plt.grid(True)\n",
    "    plt.pause(0.001)\n",
    "#     camera.snap()\n",
    "    if ind>=len(refer_path_psi)-1:\n",
    "        break\n",
    "# animation = camera.animate()\n",
    "# animation.save('trajectory.gif')\n",
    "plt.figure(2)\n",
    "plt.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0)\n",
    "plt.plot(x_, y_, 'r')\n",
    "plt.show()\n"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "0c7484b3574347463e16b31029466871583b0d4e5c4ad861e8848f2d3746b4de"
  },
  "kernelspec": {
   "display_name": "Python 3.8.12 ('gobigger')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
